/*
静态坐标变换发布方实现:雷达的坐标和旋转相对于小车中心的数据，进行话题方式发送。
关键点：
1.创建静态坐标转换的发布者对象
2.填充坐标变换的数据
3.发布数据
*/

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_broadcast");
    //创建发布者对象
    tf2_ros::StaticTransformBroadcaster pub;
    //创建坐标变换信息
    geometry_msgs::TransformStamped tfs_data;
    //设置Header数据
    tfs_data.header.seq = 100;
    tfs_data.header.stamp = ros::Time::now();
    tfs_data.header.frame_id = "base_link";
    //设置子坐标系
    tfs_data.child_frame_id = "laser";
    //设置雷达相对于小车的坐标变换
    tfs_data.transform.translation.x = 0.2;
    tfs_data.transform.translation.y = 0.0;
    tfs_data.transform.translation.z = 0.5;
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, 0);
    tfs_data.transform.rotation.x = qtn.getX();
    tfs_data.transform.rotation.y = qtn.getY();
    tfs_data.transform.rotation.z = qtn.getZ();
    tfs_data.transform.rotation.w = qtn.getW();
    //发布消息
    pub.sendTransform(tfs_data);
    ros::spin();
    return 0;
}
